/*
 * PositionManager.cpp
 *
 *  Created on: Mar 11, 2010
 *      Author: dcrown
 */

#include "PositionManager.h"
#include "BasicWindow.h"
#include "Logger.h"
#include <math.h>

extern BasicWindow* gpBW;

const float RADS_PER_DEG = 0.0174532925f;

void PositionManager::Create(MovePattern* mp, int widthOffset, int heightOffset)
{
	this->mp = mp;
	this->widthOffset = widthOffset;
	this->heightOffset = heightOffset;
}

void PositionManager::Create(int x, int y, MovePattern* mp, int widthOffset, int heightOffset)
{
	Create(mp, widthOffset, heightOffset);
	SetAllPositions(x, y);
}

void PositionManager::Destroy()
{
	delete mp;
}

void PositionManager::Clear()
{
	centerX = centerY = -1;
	offsetX = offsetY = -1;
	bearingStartX = bearingStartY = -1;
	widthOffset = heightOffset = -1;

	currSpeed_ups = 0;
	currBearing_deg = 90;
	changeTime = -1;
	movePatternPos = -1;
	running = false;
}

void PositionManager::Start()
{
	running = true;
	StepMovePattern();
	moveClock.Start();
}

void PositionManager::StepMovePattern()
{

	//Logger::getInstance().LOG(Logger::DEBUG, "movePatternPos: %d", movePatternPos);
	movePatternPos++;
	//Logger::getInstance().LOG(Logger::DEBUG, "mp=%d, pos=%d", &mp, movePatternPos);
	currSpeed_ups = mp->GetChangeSpeed(movePatternPos);
	//Logger::getInstance().LOG(Logger::DEBUG, "2: %d", movePatternPos);
	currBearing_deg = mp->GetChangeBearing(movePatternPos);
	//Logger::getInstance().LOG(Logger::DEBUG, "3: %d", movePatternPos);
	changeTime = mp->GetChangeTime(movePatternPos);
	//Logger::getInstance().LOG(Logger::DEBUG, "4: %d", movePatternPos);
	moveClock.Mark("mark", Clock::UPDATE_MARK);
	SetBearingStartPos();

}

void PositionManager::Update()
{
	//Logger::getInstance().LOG(Logger::DEBUG, "Update");
	static bool displayNext = false;
	if (running)
	{
		float deltaT = moveClock.GetTimeSinceMark("mark", Clock::LEAVE_MARK);
		float totalDistance = currSpeed_ups*deltaT/1000;

		if (currBearing_deg <= 90)
		{
			centerX = bearingStartX+totalDistance*cos(currBearing_deg*RADS_PER_DEG);
			centerY = bearingStartY-totalDistance*sin(currBearing_deg*RADS_PER_DEG);
		}
		else if (currBearing_deg <= 180)
		{
			centerX = bearingStartX-totalDistance*sin((currBearing_deg-90)*RADS_PER_DEG);
			centerY = bearingStartY-totalDistance*cos((currBearing_deg-90)*RADS_PER_DEG);
		}
		else if (currBearing_deg <= 270)
		{
			centerX = bearingStartX-totalDistance*cos((currBearing_deg-180)*RADS_PER_DEG);
			centerY = bearingStartY+totalDistance*sin((currBearing_deg-180)*RADS_PER_DEG);
		}
		else
		{
			centerX = bearingStartX+totalDistance*sin((currBearing_deg-270)*RADS_PER_DEG);
			centerY = bearingStartY+totalDistance*cos((currBearing_deg-270)*RADS_PER_DEG);
		}

		if (displayNext)
		{
			Logger::getInstance().LOG(Logger::DEBUG, "center: (%f, %f)", centerX, centerY);
			displayNext = false;
		}

		SetOffsetPos();

		if ((changeTime >= 0) && (deltaT >= changeTime))
		{
			//Logger::getInstance().LOG(Logger::DEBUG, "delatT: %f, changeTime: %d", deltaT, changeTime);
			//Logger::getInstance().LOG(Logger::DEBUG, "center: (%f, %f)", centerX, centerY);
			displayNext = true;
			StepMovePattern();
		}
	}
}

/*
void PositionManager::RandomChangeBearing(int spread)
{
	bearing_deg = (bearing_deg + (rand() % spread*2)-spread)%360;
	StartNewBearing();
}*/

//-----------------------------------------------------------------------------------------------------------------------------------
// Set Positions
//-----------------------------------------------------------------------------------------------------------------------------------
void PositionManager::SetAllPositions(float x, float y)
{
	//Logger::getInstance().LOG(Logger::DEBUG, "SetAllPos: (%f, %f)", x, y);
	SetCenterPos(x, y);
	SetOffsetPos();
	SetBearingStartPos();
	//Logger::getInstance().LOG(Logger::DEBUG, "SetAllPos: (%f, %f)", x, y);
}

void PositionManager::SetCenterPos(float x, float y)
{
	centerX = x;
	centerY = y;
}

void PositionManager::SetBearingStartPos()
{
	//Logger::getInstance().LOG(Logger::DEBUG, "SetBearingStartPos");
	bearingStartX = centerX;
	bearingStartY = centerY;
	//Logger::getInstance().LOG(Logger::DEBUG, "SetBearingStartPos: (%f, %f)", bearingStartX, bearingStartY);
}

void PositionManager::SetOffsetPos()
{
	if (OffsetDimsValid())
	{
		offsetX = (int)centerX - widthOffset/2;
		offsetY = (int)centerY - heightOffset/2;
	}
}

bool PositionManager::OffsetDimsValid()
{
	return ((widthOffset != -1) && (heightOffset != -1));
}
